// charge_controller.hpp
#pragma once

#include <rclcpp/rclcpp.hpp>
#include <cmath>
#include <std_msgs/msg/float64.hpp>
#include <agv_msgs/msg/read_in_puts.hpp>
#include <agv_msgs/msg/read_in_puts_new.hpp>
#include <agv_srvs/srv/output_interface.hpp>
#include <agv_srvs/srv/outputs_new.hpp>
#include <agv_msgs/msg/out_puts_new.hpp>
#include <agv_msgs/msg/output_port.hpp>
#include <agv_msgs/msg/agv_event_status.hpp>
#include <agv_srvs/srv/battery_charge.hpp>
#include <agv_msgs/msg/agv_mode.hpp>
#include <agv_msgs/msg/periph_teleop.hpp>
#include <agv_msgs/msg/agv_info.hpp>
#include <agv_msgs/msg/pair_type_int.hpp>
#include <agv_msgs/msg/agv_auto_status.hpp>
#include <can_msgs/msg/frame.hpp>

#include <sys/socket.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <unistd.h>
#include <cstring>  
#include <linux/can.h> 

#if defined(HAVE_LIBSOCKETCAN)
#include <libsocketcan/can/raw.h>
#include <linux/can/raw.h>
#endif

#include <thread>
#include <mutex>
#include <chrono>
#include <vector>
#include <functional>
#include "event_manager.hpp"
#include "toolKit.hpp"

#define CHARGING_NAME  "auto_charging"
#define TOPIC_TIMEOUT_MS 500
#define CMD_TIMEOUT_MS 10000
#define DEFAUT_POINT_ID 65535

using namespace std::chrono_literals;
using TimePoint = std::chrono::steady_clock::time_point;

class ChargeController {
public:
    enum state_type {
        IDLE = 0u,
        Fall = 5u,
        SELECT = 9u,
        CHARGING_ACTION = 10u,
        CHARGING_ONPOINT = 15u,
        WAITING_Timeout = 20u,
        WAITING_Inplace = 30u,
        Raise = 40u,
        ERROR = 99u,
        OVER = 100u,
    };
    
    enum periphTeleopBut {
        NO_ACTIVE = 0,
        UP,
        DOWN,
        CLOCKWISE_ROTATION,
        ANTICLOCKWISE_ROTAION
    };
    
    struct read_input {
        bool high_pos; 
        bool low_pos;
        bool resetBtn;
        bool retractInplaceSensor;
        read_input() : 
            high_pos(false),
            low_pos(false),
            resetBtn(false),
            retractInplaceSensor(false) {}
    };

    ChargeController();
    ~ChargeController();
    
    state_type get_state() { return state_; }
    bool isManual();
    bool OnChargePoint();
    void set_charge(bool state);
    void init(std::shared_ptr<rclcpp::Node> node);
    
private:  
    void setOutputNew();
    void callOutputSrv(const std::string& outStr, bool needOpen);
    void inputState_get(const agv_msgs::msg::ReadInPutsNew &msg, const std::string& key, bool &state);
    void platePosCheck();
    void batteryHeat();
    void thread_cycle();
    void liftDriver_thread_cycle();
    void thread_fun();
    void liftDriver_thread_fun();
    
    // Callbacks
    void agvmodeCallback(const agv_msgs::msg::AGVMode::SharedPtr state_p);
    void auto_statusCallback(const agv_msgs::msg::AgvAutoStatus::SharedPtr msg);
    void periphTeleopCallback(const agv_msgs::msg::PeriphTeleop::SharedPtr msg);
    void eventCallback(const agv_msgs::msg::AGVEventStatus::SharedPtr event);
    void inputCallback(const agv_msgs::msg::ReadInPutsNew::SharedPtr msg);
    void chargeCB(
        const agv_srvs::srv::BatteryCharge::Request::SharedPtr req,
        agv_srvs::srv::BatteryCharge::Response::SharedPtr resp);
    
    bool timer_ON(bool level, int target_ms);
    void setOnePoint(bool level, const std::string& key);
    void clearOutputs();
    bool outputsPub();
    
    // ROS2 members
    rclcpp::Publisher<agv_msgs::msg::OutPutsNew>::SharedPtr io_pub_;
    rclcpp::Subscription<agv_msgs::msg::AGVMode>::SharedPtr agvmode_sub_;
    rclcpp::Subscription<agv_msgs::msg::AGVEventStatus>::SharedPtr eventOccurSub_;
    rclcpp::Subscription<agv_msgs::msg::ReadInPutsNew>::SharedPtr inputNewSub_;
    rclcpp::Subscription<agv_msgs::msg::AgvAutoStatus>::SharedPtr auto_run_sub_;
    rclcpp::Subscription<agv_msgs::msg::PeriphTeleop>::SharedPtr periphTeleop_sub_;
    rclcpp::Service<agv_srvs::srv::BatteryCharge>::SharedPtr trigger_server_;
    rclcpp::Client<agv_srvs::srv::OutputsNew>::SharedPtr outputclientNew_;
    
    // Threading
    std::thread thread_, liftDriver_thread_;
    std::mutex quit_mutex_, liftDriver_quit_mutex_;
    std::atomic<bool> running_{true};
    
    // State variables
    state_type state_ = IDLE;
    agv_msgs::msg::AGVMode mode_;
    agv_msgs::msg::AGVEventStatus event_;
    
    bool start_ = false;
    bool OnChargePoint_ = false;
    bool chargeRelay_ = false;
    bool chargeEnable_ = false;
    bool ending_ = false;
    bool retractSensorEnable_ = false;
    bool batteryHeating_ = false;
    bool is_batteryheating_ = false;
    bool plateRaiseReq_ = false;
    bool plateAutoRaiseReq_ = false;
    bool plateFallReq_ = false;
    bool chargePlateNeedFall_ = false;
    bool lift_timeout_ = false;
    
    read_input inputs_;
    std::string battery_type_;
    int can_socket_ = -1;
    
    // Automatic operation
    CDataTimeStamp<uint8_t> teleopCode_;
    CTimeStamp cmdValid_;
    CEventManager eventManger_;
    int target_ms_ = 5000;
    int retractInPlace_pt_ms_ = 500;
    int bit_index_ = -1;
    
    std::vector<agv_msgs::msg::PairTypeInt> outPuts_;
    agv_msgs::msg::AgvAutoStatus auto_run_;

    std::shared_ptr<rclcpp::Node> node_;
};